clear all; 
clc;
d1=0.25; d4=0.25;  %base and toolpoint transplante
a2=0.21; a3=0.165; a4=0.21;
alpha2=pi/2;  alpha6=pi/2;

% syms theta1 theta2 theta3 theta4 theta5 theta6 real
% q={theta1,theta2,theta3,theta4,theta5,theta6};
theta1=0; theta2=pi/3; theta3=-pi/3; theta4=-pi/3; theta5=pi/3; theta6=0;

T10=[       cos(theta1),      -sin(theta1),      0,        0;
    sin(theta1)*cos(-pi/2),cos(theta1)*cos(-pi/2),-sin(-pi/2),-sin(-pi/2)*0;
    sin(theta1)*sin(-pi/2),cos(theta1)*sin(-pi/2), cos(-pi/2), cos(-pi/2)*0;
                     0,                 0,      0,         1
    ]
T21=[       cos(theta2),      -sin(theta2),      0,        0;
    sin(theta2)*cos(pi/2),cos(theta2)*cos(pi/2),-sin(pi/2),-sin(pi/2)*0;
    sin(theta2)*sin(pi/2),cos(theta2)*sin(pi/2), cos(pi/2), cos(pi/2)*0;
                     0,                 0,      0,         1
    ];
T32=[       cos(theta3),      -sin(theta3),      0,       a2;
    sin(theta3)*cos(0),cos(theta3)*cos(0),-sin(0),-sin(0)*0;
    sin(theta3)*sin(0),cos(theta3)*sin(0), cos(0), cos(0)*0;
                     0,                 0,      0,         1
    ];
T43=[       cos(theta4),      -sin(theta4),      0,       a3;
    sin(theta4)*cos(0),cos(theta4)*cos(0),-sin(0),-sin(0)*0;
    sin(theta4)*sin(0),cos(theta4)*sin(0), cos(0), cos(0)*0;
                     0,                 0,      0,         1
    ];
T54=[       cos(theta5),      -sin(theta5),      0,       a4;
    sin(theta5)*cos(0),cos(theta5)*cos(0),-sin(0),-sin(0)*0;
    sin(theta5)*sin(0),cos(theta5)*sin(0), cos(0), cos(0)*0;
                     0,                 0,      0,         1
    ];
T65=[       cos(theta6),      -sin(theta6),      0,        0;
    sin(theta6)*cos(pi/2),cos(theta6)*cos(pi/2),-sin(pi/2),-sin(pi/2)*0;
    sin(theta6)*sin(pi/2),cos(theta6)*sin(pi/2), cos(pi/2), cos(pi/2)*0;
                     0,                 0,      0,         1
    ];
 
%%
%Matrix compute
% Tb=[1,0,0,0;0,1,0,0;0,0,1,d1;0,0,0,1];   %respect to basement transform matrix
% T10=Tb*T10;
T20=T10*T21;
T30=T20*T32;
T40=T30*T43;
T50=T40*T54;
T10
T20
T30
T40
T50
T60=T50*T65;
 
%%
%number compute
% qz={0,0,0,0,0,0}; %ready state 
% T6=vpa(subs(T60,q,qz),3)  %compute simplify
disp('output transform matrix of the endpoint respect to basement T6:')
T60
% qu={0 pi/3 -pi/3 -pi/3 pi/3 0};%standup state
% T6=vpa(subs(T60,q,qz),3)
 
%%
%draw the posture of robot
x = [T10(1,4) T20(1,4) T30(1,4) T40(1,4) T50(1,4) T60(1,4)]
y = [T10(2,4) T20(2,4) T30(2,4) T40(2,4) T50(2,4) T60(2,4)]
z = [T10(3,4) T20(3,4) T30(3,4) T40(3,4) T50(3,4) T60(3,4)]
 
%draw the toolpoint coordinate
px=T60*[0.05;0;0;1];     py=T60*[0;0.05;0;1];     pz=T60*[0;0;0.05;1]; %'50'is properties of coordinate
px1=[T60(1,4),px(1,1)];py1=[T60(2,4),px(2,1)];pz1=[T60(3,4),px(3,1)];
px2=[T60(1,4),py(1,1)];py2=[T60(2,4),py(2,1)];pz2=[T60(3,4),py(3,1)];
px3=[T60(1,4),pz(1,1)];py3=[T60(2,4),pz(2,1)];pz3=[T60(3,4),pz(3,1)];
%%
%drawing figures
plot3(x,y,z,'o','linewidth',8);
hold on 
%set coordinate
plot3(px1,py1,pz1,'r','LineWidth',3)
hold on 
plot3(px2,py2,pz2,'g','LineWidth',3)
hold on 
plot3(px3,py3,pz3,'b','LineWidth',3)
title("Forward Kinematics")
xlabel("x(m)")
ylabel("y(m)")
zlabel("z(m)")
plot3(x,y,z, 'y','Linewidth',5);
grid on;
